// This file is part of MatrixPilot.
//
//    http://code.google.com/p/gentlenav/
//
// Copyright 2009, 2010 MatrixPilot Team
// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
//
// MatrixPilot is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MatrixPilot is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MatrixPilot.  If not, see <http://www.gnu.org/licenses/>.


#include "libUDB_internal.h"

#if (BOARD_IS_CLASSIC_UDB == 1)

//	Measure the pulse widths of the servo channel inputs from the radio.
//	The dsPIC makes this rather easy to do using its capture feature.

//	One of the channels is also used to validate pulse widths to detect loss of radio.

//	The pulse width inputs can be directly converted to units of pulse width outputs to control
//	the servos by simply dividing by 2.

#define PPM_NUMBER_OF_CHANNELS 8

int udb_pwIn[MAX_INPUTS+1] ;	// pulse widths of radio inputs
int udb_pwTrim[MAX_INPUTS+1] ;	// initial pulse widths for trimming

int failSafePulses = 0 ;

unsigned int rise[MAX_INPUTS+1] ;	// rising edge clock capture for radio inputs

unsigned char ch 		= 0;		// PPM channel counter
unsigned int  lastRise 	= 0;		// record PPM rise time

boolean	frameOK;					// Are all measurements in the frame ok?
	
#define PPM_FAILSAFE_INPUT_MIN					1500

// WHY?
#define PPM_ADJUST								1000

//Number to ajust the PPM inputs to numbers equal the PWM input 
//So if you read from without adjust 840 to 3050 you might want to add 1000 to the input reading
//To adjust to the normal 2000 to 4000 PWM input
#define PPM_ADJUST								1000



void udb_init_capture(void)
{
	T2CON = 0b1000000000000000  ;	// turn on timer 2 with no prescaler
	TRISD = 0b1111111111111111 ;	// make the d port input, to enable IC1 and IC2
	TRISFbits.TRISF6 = 1 ;			// make F6 an input to enable the 3rd switch
	IC1CON = IC2CON = IC7CON = IC8CON = 0b0010000010000011 ;	//IMC<2:0> = 011 (Capture every rising edge)
		
	IPC0bits.IC1IP = IPC1bits.IC2IP = IPC4bits.IC7IP = IPC4bits.IC8IP = 6 ; // priority 6
	IFS0bits.IC1IF = IFS0bits.IC2IF = IFS1bits.IC7IF = IFS1bits.IC8IF = 0 ; // clear the interrupt
	
//	_IC7IE = 1 ; // turn on interrupt for input 1
	_IC1IE = 1 ; // turn on interrupt for input 4 - Leaves two ADC channels free.

	return ;
}


// Input Channel 1
void __attribute__((__interrupt__,__no_auto_psv__)) _IC7Interrupt(void)
{
	unsigned int time ;
	unsigned int pulse;
	
	IFS1bits.IC7IF = 0 ; // clear the interrupt

	while ( IC7CONbits.ICBNE )
	{
		time = IC7BUF ;
	}
	
#if ( NORADIO == 0 )
	
	pulse = ((time - lastRise ) >> 1 );

	if (pulse > 10000) 			//sync pulse
	{ 								
		ch = 1;
		frameOK	= true;
	}
	else
	{
		udb_pwIn[ch] = pulse;
		
		if (ch < PPM_NUMBER_OF_CHANNELS + 1 ) 
			ch++;		//scan next channel
		else 
		{
			if(frameOK	== false)
			{
				failSafePulses = 0 ;
				udb_flags._.radio_on = 0 ;
				LED_GREEN = LED_OFF ;
			}

			if( (udb_pwIn[FAILSAFE_INPUT_CHANNEL] > FAILSAFE_INPUT_MIN) && (udb_pwIn[FAILSAFE_INPUT_CHANNEL] < FAILSAFE_INPUT_MAX ) )
				failSafePulses++ ;

			ch = 1;									//reset
			frameOK	= true;
		}

		if ( (pulse < PPM_FAILSAFE_INPUT_MIN ) )
		{
			frameOK	= false;
		}
	}

	lastRise = time;

#endif
}



// Input Channel 4
void __attribute__((__interrupt__,__no_auto_psv__)) _IC1Interrupt(void)
{
	unsigned int time ;
	unsigned int pulse;

	_IC1IF =  0 ; // clear the interrupt

	while ( IC1CONbits.ICBNE )
	{
		time = IC1BUF ;
	}
	
#if ( NORADIO == 0 )
	
	pulse = ((time - lastRise ) >> 1 );

	if (pulse > 10000) 			//sync pulse
	{ 								
		ch = 1;
		frameOK	= true;
	}
	else
	{
		udb_pwIn[ch] = pulse;
		
		if (ch < PPM_NUMBER_OF_CHANNELS + 1 ) 
			ch++;		//scan next channel
		else 
		{
			if(frameOK	== false)
			{
				failSafePulses = 0 ;
				udb_flags._.radio_on = 0 ;
				LED_GREEN = LED_OFF ;
			}

			if( (udb_pwIn[FAILSAFE_INPUT_CHANNEL] > FAILSAFE_INPUT_MIN) && (udb_pwIn[FAILSAFE_INPUT_CHANNEL] < FAILSAFE_INPUT_MAX ) )
				failSafePulses++ ;

			ch = 1;									//reset
			frameOK	= true;
		}

		if ( (pulse < PPM_FAILSAFE_INPUT_MIN ) )
		{
			frameOK	= false;
		}
	}

	lastRise = time;

#endif
	
	return ;
}


#endif	//(BOARD_IS_CLASSIC_UDB == 1)
